/*!
  @file feature_matcher.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include "mrover_vo/feature_matcher.h"

#include <iterator>
#include <limits>
#include <map>
#include <vector>

#include <ros/ros.h>

using std::vector;

namespace vo {

void FeatureMatcher::compute() {
  if (!initCompute()) {
    ROS_ERROR("Error in feature matching");
    matches_->clear();
    return;
  }

  matches_->clear();
  radiusMatch(200);
  //matcher_->match(*input_desc_, *target_desc_, *matches_);
   ROS_INFO("kpL %d, kpR %d, match %d", input_kp_->size(), target_kp_->size(), matches_->size());
  //filterWeakMatches();
  checkMutualConsistency();
  checkForValidity();
  // ROS_INFO("kpL %d, kpR %d, match %d (after filtering invalid matches)", input_kp_->size(), target_kp_->size(), matches_->size());
}

void FeatureMatcher::radiusMatch(const float MAX_RADIUS) {
  cv::Mat mask = cv::Mat::zeros(input_desc_->rows, target_desc_->rows, CV_8U);
  for (size_t i = 0; i < input_kp_->size(); ++i) {
    for (size_t j = 0; j < target_kp_->size(); ++j) {
      if (cv::norm(input_kp_->at(i).pt - target_kp_->at(j).pt) < MAX_RADIUS)
          mask.at<uchar>(i, j) = 1;
    }
  }
  matches_->clear();
  matcher_->match(*input_desc_, *target_desc_, *matches_, mask);
}

void FeatureMatcher::filterWeakMatches() {
  // calculate max/min distance 
  float max_dist = std::numeric_limits<float>::min(), 
        min_dist = std::numeric_limits<float>::max();
  for (Feature::Matches::iterator match_it = matches_->begin(); match_it != matches_->end(); match_it++) {
    float dist = match_it->distance;
    if (dist < min_dist && dist > 0) min_dist = dist;
    if (dist > max_dist && dist > 0) max_dist = dist;
  }

  // remove matches which has > 3*min_dist distance
  Feature::Matches::iterator match_it = matches_->begin();
  while (match_it != matches_->end()) {
    if (match_it->distance > 3 * min_dist) {
      match_it = matches_->erase(match_it);
    } else {
      ++match_it;
    }
  }
}

void FeatureMatcher::checkMutualConsistency() {
  int input_sz  = static_cast<int>(input_kp_->size());
  int target_sz = static_cast<int>(target_kp_->size());
  float fmax = std::numeric_limits<float>::max();

  cv::Mat cost(input_sz, target_sz, CV_32F, fmax);
  vector<int> map_match(input_sz, -1);

  for (size_t i = 0; i < matches_->size(); ++i) {
    cv::DMatch &m = matches_->at(i);
    if (m.distance < cost.at<float>(m.queryIdx, m.trainIdx)) {
      cost.at<float>(m.queryIdx, m.trainIdx) = m.distance;
      map_match[m.queryIdx] = i;
    }
  }

  vector<int> input_idx(input_kp_->size(), -1);
  vector<int> target_idx(target_kp_->size(), -1);

  double minVal = 0.0;
  cv::Point minLoc(-1, -1);
  for (size_t i = 0; i < input_kp_->size(); ++i) {
    cv::minMaxLoc(cost.row(i), &minVal, NULL, &minLoc, NULL);
    if (minVal < fmax) input_idx[i] = minLoc.x;
  }
  for (size_t i = 0; i < target_kp_->size(); ++i) {
    cv::minMaxLoc(cost.col(i), &minVal, NULL, &minLoc, NULL);
    if (minVal < fmax) target_idx[i] = minLoc.y;
  }

  Feature::Matches tmp_matches;
  tmp_matches.swap(*matches_);
  for (size_t i = 0; i < input_kp_->size(); ++i) {
    if (static_cast<int>(i) == target_idx[input_idx[i]]) matches_->push_back(tmp_matches[map_match[i]]);
  }
}

void FeatureMatcher::checkForValidity() {
  vector<ValidityCheckerConstPtr>::iterator vc_it;
  for (vc_it = validity_checker_.begin(); vc_it != validity_checker_.end(); ++vc_it) {
    (*vc_it)->checkForValidityWithErase(this);
  }
}

}  // namespace vo
